#include "fisheyecalibrate.h"

FisheyeCalibrate::FisheyeCalibrate()
{
    board_size = Size(10,7);            //定标板上每行、列的角点数
    success_image_num = 0;				//成功提取角点的棋盘图数量
    x_expand = 0;
    y_expand = 200;		//x,y方向的扩展(x横向，y纵向)，适当增大可以不损失原图像信息
}

//Step 1: extract corner points in each images
int FisheyeCalibrate::extractCornerPoint()
{
    /************************************************************************
           读取每一幅图像，从中提取出角点，然后对角点进行亚像素精确化
    *************************************************************************/
    cout << "Start extracting corner points ..." << endl;
    //以下三行为需要手动修改的参数
    image_count=  14;                   //图像数量


    vector<Point2f> corners;                //缓存每幅图像上检测到的角点
    corners_Seq.clear();   //保存检测到的所有角点/
    image_Seq.clear();
    success_image_num = 0;

    bool conner_flag = true;				//所有图像角点提取成功为true，其余为false
    for( int i = 0;  i != image_count ; i++)
    {
        cout<<"img"<<i+1<<"..."<<endl;
        string imageFileName;
        std::stringstream StrStm;
        StrStm<<i+1;
        StrStm>>imageFileName;
        imageFileName += ".jpg";
        cv::Mat imageSrc = imread("../images/img"+imageFileName);
        Mat image;//边界扩展后的图片
        copyMakeBorder(imageSrc,image,(int)(y_expand/2),(int)(y_expand/2),(int)(x_expand/2),(int)(x_expand/2),BORDER_CONSTANT);
        /* 提取角点 */
        Mat imageGray;
        cvtColor(image, imageGray , CV_RGB2GRAY);
        bool patternfound = findChessboardCorners(image, board_size, corners,CALIB_CB_ADAPTIVE_THRESH + CALIB_CB_NORMALIZE_IMAGE+
                                                  CALIB_CB_FAST_CHECK );
        if (!patternfound)
        {
            cout<<"img"<<i+1<<" corner points extraction failed. "
                              "Please delete the image and re run the program after reordering."<<endl;
            conner_flag = false;
            break;
        }
        else
        {
            /* 亚像素精确化 */
            cornerSubPix(imageGray, corners, Size(11, 11), Size(-1, -1), TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1));
            /* 绘制检测到的角点并保存 */
            Mat imageTemp = image.clone();
            for (int j = 0; j < corners.size(); j++)
            {
                circle( imageTemp, corners[j], 10, Scalar(0,0,255), 2, 8, 0);
            }
            string imageFileName;
            std::stringstream StrStm;
            StrStm<<i+1;
            StrStm>>imageFileName;
            imageFileName += "_corner.jpg";
            imwrite(imageFileName,imageTemp);
            cout<<"img"<<i+1<<"'s corner points extraction done."<<endl;

            success_image_num = success_image_num + 1;
            corners_Seq.push_back(corners);
        }
        image_Seq.push_back(image);
    }
    if (!conner_flag)//如果有提取失败的标定图，退出程序
    {
        cout<<"Some extraction failed, exiting ..."<<endl;
        return -1;
    }
    cout<<"All corner points extraction finished!"<<endl;
    return 0;
}

//Step 2: calibrate camera
int FisheyeCalibrate::calibrateCamera()
{
    /************************************************************************
               摄像机定标
    *************************************************************************/
    cout<<"Start calibration ..."<<endl;
    Size square_size = Size(20,20);
    object_Points.clear();        /****  保存定标板上角点的三维坐标   ****/
    point_counts.clear();

    /* 初始化定标板上角点的三维坐标 */
    for (int t = 0; t<success_image_num; t++)
    {
        vector<Point3f> tempPointSet;
        for (int i = 0; i<board_size.height; i++)
        {
            for (int j = 0; j<board_size.width; j++)
            {
                /* 假设定标板放在世界坐标系中z=0的平面上 */
                Point3f tempPoint;
                tempPoint.x = i*square_size.width;
                tempPoint.y = j*square_size.height;
                tempPoint.z = 0;
                tempPointSet.push_back(tempPoint);
            }
        }
        object_Points.push_back(tempPointSet);
    }
    for (int i = 0; i< success_image_num; i++)
    {
        point_counts.push_back(board_size.width*board_size.height);
    }
    /* 开始定标 */
    Size image_size = image_Seq[0].size();

    int flags = 0;
    flags |= cv::fisheye::CALIB_RECOMPUTE_EXTRINSIC;
    flags |= cv::fisheye::CALIB_CHECK_COND;
    flags |= cv::fisheye::CALIB_FIX_SKEW;
    fisheye::calibrate(object_Points, corners_Seq, image_size, intrinsic_matrix, distortion_coeffs, rotation_vectors, translation_vectors, flags, cv::TermCriteria(3, 20, 1e-6));
    cout<<"Calibration finished!"<<endl;
    return 0;
}

//Step 3: evaluate calibration
int FisheyeCalibrate::evalCalibration()
{
    /************************************************************************
               对定标结果进行评价
    *************************************************************************/
    cout<<"Calibration reaults evaluation started ..."<<endl;
    fout.open("caliberation_result.txt");  /**    保存定标结果的文件     **/
    double total_err = 0.0;                   /* 所有图像的平均误差的总和 */
    double err = 0.0;                        /* 每幅图像的平均误差 */
    vector<Point2f>  image_points2;             /****   保存重新计算得到的投影点    ****/

    cout<<"Calibration error for every image:"<<endl;
    fout<<"Calibration error for every image:"<<endl<<endl;
    for (int i=0;  i<image_count;  i++)
    {
        vector<Point3f> tempPointSet = object_Points[i];
        /****    通过得到的摄像机内外参数，对空间的三维点进行重新投影计算，得到新的投影点     ****/
        fisheye::projectPoints(tempPointSet, image_points2, rotation_vectors[i], translation_vectors[i], intrinsic_matrix, distortion_coeffs);
        /* 计算新的投影点和旧的投影点之间的误差*/
        vector<Point2f> tempImagePoint = corners_Seq[i];
        Mat tempImagePointMat = Mat(1,tempImagePoint.size(),CV_32FC2);
        Mat image_points2Mat = Mat(1,image_points2.size(), CV_32FC2);
        for (size_t i = 0 ; i != tempImagePoint.size(); i++)
        {
            image_points2Mat.at<Vec2f>(0,i) = Vec2f(image_points2[i].x, image_points2[i].y);
            tempImagePointMat.at<Vec2f>(0,i) = Vec2f(tempImagePoint[i].x, tempImagePoint[i].y);
        }
        err = norm(image_points2Mat, tempImagePointMat, NORM_L2);
        total_err += err/=  point_counts[i];
        cout<<"The "<<i+1<<" image's average error: "<<err<<" pixels"<<endl;
        fout<<"The "<<i+1<<" image's average error: "<<err<<" pixels"<<endl;
    }
    cout<<"Total average error: "<<total_err/image_count<<" pixels"<<endl;
    fout<<"Total average error: "<<total_err/image_count<<" pixels"<<endl<<endl;
    cout<<"Evaluation finished!"<<endl;
    return 0;
}

//Step 4.1: save calibration result
int FisheyeCalibrate::saveCalibration()
{
    /************************************************************************
               保存定标结果
    *************************************************************************/
    cout<<"Saving calibration result ..."<<endl;
    Mat rotation_matrix = Mat(3,3,CV_32FC1, Scalar::all(0)); /* 保存每幅图像的旋转矩阵 */

    fout<<"Camera Internal parameter matrix:"<<endl;
    fout<<intrinsic_matrix<<endl;
    fout<<"Distortion coefficient:"<<endl;
    fout<<distortion_coeffs<<endl;
    for (int i=0; i<image_count; i++)
    {
        fout<<"The "<<i+1<<" image's rotation vector:"<<endl;
        fout<<rotation_vectors[i]<<endl;

        /* 将旋转向量转换为相对应的旋转矩阵 */
        Rodrigues(rotation_vectors[i],rotation_matrix);
        fout<<"The "<<i+1<<" image's rotation array:"<<endl;
        fout<<rotation_matrix<<endl;
        fout<<"The "<<i+1<<" image's translation vector:"<<endl;
        fout<<translation_vectors[i]<<endl;
    }
    cout<<"Saved!"<<endl;
    fout<<endl;
    return 0;
}

//Step 4.2: load calibration result
int FisheyeCalibrate::loadCalibration()
{
    return 0;
}

//Step 5: show the calibration result
int FisheyeCalibrate::showCalibration()
{
    /************************************************************************
           显示定标结果
    *************************************************************************/
    Size image_size = image_Seq[0].size();
    mapx = Mat(image_size,CV_32FC1);
    mapy = Mat(image_size,CV_32FC1);
    R = Mat::eye(3,3,CV_32F);
    cout<<"Saving undistorted image"<<endl;
    for (int i = 0 ; i != image_count ; i++)
    {
        fisheye::initUndistortRectifyMap(intrinsic_matrix,distortion_coeffs,R,intrinsic_matrix,image_size,CV_32FC1,mapx,mapy);
        Mat t = image_Seq[i].clone();
        cv::remap(image_Seq[i],t,mapx, mapy, INTER_LINEAR);
        string imageFileName;
        std::stringstream StrStm;
        StrStm<<i+1;
        StrStm>>imageFileName;
        imageFileName += "_d.jpg";
        imwrite(imageFileName,t);
        //imshow(imageFileName, t);
        cout<<"img"<<i+1<<" saved"<<endl;
    }
    cout<<"All saved!"<<endl;
    return 0;
}

//Step 6: undistort image with the calibration
int FisheyeCalibrate::undistortImage(std::string filename)
{
    cout<<"TestImage ..."<<endl;
    Size image_size = image_Seq[0].size();
    Mat image = imread(filename,1);
    Mat testImage;
    copyMakeBorder(image,testImage,(int)(y_expand/2),(int)(y_expand/2),(int)(x_expand/2),(int)(x_expand/2),BORDER_CONSTANT);
    fisheye::initUndistortRectifyMap(intrinsic_matrix,distortion_coeffs,R,intrinsic_matrix,image_size,CV_32FC1,mapx,mapy);
    Mat t = testImage.clone();
    cv::remap(testImage,t,mapx, mapy, INTER_LINEAR);

    imwrite("TestOutput.jpg",t);
    //imshow("TestOutput.jpg", t);
    cout<<"Saved!"<<endl;
    return 0;
}
